/**
 * @brief augmavconn class interface
 * @file augmavconn.cpp
 * @author Lai Jun <laijun@nudt.edu.cn>
 *
 * @brief augmavconn an advanced mavconn interface with source management strategy
 *
 */
#include <memory>
#include "stdio.h"
#include "augmavconn.h"
#include "fifo/fifoconnector.h"
#include "fifo/message_fifo.h"
#include "fifo/mTimer.h"

/*#include <sys/types.h>
#include <unistd.h>
#include <sys/syscall.h>

#define gettid() syscall(__NR_gettid)*/

uint8_t flag_Aperiodic=0;
extern uint8_t flag_Time;
extern mTimer * Common_Timer;

struct timeval tv;
struct timezone tz;
struct tm *p;

Mes_Time message_time;

using namespace augmavconn;

AugMavConnInterface::AugMavConnInterface(MAVConnInterface::Ptr mavconnInterface){
    conn = new FiFoConnector;
    stdMavconnInterface = mavconnInterface;
    stdMavconnInterface->message_received.connect(this->message_received);
    stdMavconnInterface->port_closed.connect(this->port_closed);
    SetDataHandler(this);
    // some other codes for FIFO initial
}

AugMavConnInterface::AugPtr AugMavConnInterface::open_url(std::string url,
			uint8_t system_id , uint8_t component_id ){
        MAVConnInterface::Ptr mavconnInterface = MAVConnInterface::open_url(url,
		system_id,component_id);
        //TODO is there any problem
        return boost::make_shared<AugMavConnInterface>(mavconnInterface);
}

void AugMavConnInterface::send_message( mavlink_message_t *message){
    uint8_t msgid= getMavMsgId(message);

    if(msgid==PARAM_VALUE||msgid==MISSION_REQUEST||msgid==MISSION_COUNT||msgid==MISSION_ACK||msgid==MISSION_ITEM||msgid==COMMAND_ACK||msgid==STATUSTEXT)
    {
        flag_Aperiodic=1;
    }

    gettimeofday(&tv,&tz);
    p=localtime(&tv.tv_sec);
    message_time.second =p->tm_sec;
    message_time.msec = tv.tv_usec;
    printf("fifo in :msgid:%d,time: %d:%d\n",message->msgid,message_time.second,message_time.msec);

    conn->ArgMavConn(*message,msgid,message_time);
    
    //stdMavconnInterface -> send_message(message);
}

void AugMavConnInterface::send_message( mavlink_message_t *message, uint8_t sysid, uint8_t compid){
    // uint8_t msgid= getMavMsgId(message);
    // int msglen = getMavMessageLen(message);
    // conn->ArgMavConn((uint8_t *)message,msglen,msgid);
    // printf("fifo in：msgid:%d\n",msgid);

    uint8_t msgid= getMavMsgId(message);

    if(msgid==HEARTBEAT||msgid==SYS_STATUS||msgid==STATE_SP||msgid==PARAM_VALUE||msgid==MISSION_REQUEST||msgid==MISSION_COUNT||msgid==MISSION_ACK||msgid==MISSION_ITEM||msgid==COMMAND_ACK||msgid==STATUSTEXT)
    {
        flag_Aperiodic=1;
    }

    gettimeofday(&tv,&tz);
    p=localtime(&tv.tv_sec);
    message_time.second =p->tm_sec;
    message_time.msec = tv.tv_usec;
    printf("fifo in :msgid:%d,time: %d:%d\n",message->msgid,message_time.second,message_time.msec);

    conn->ArgMavConn(*message,msgid,message_time);
    

    //stdMavconnInterface-> send_message(message,sysid,compid);
}

void AugMavConnInterface::real_send_message( mavlink_message_t message,int id)
{

    // printf("fifo out：msgid:%d\n",id);
    // printf("message real id:%d\n",getMavMsgId((mavlink_message_t *)message));
    // printf("message pyload length id:%d\n",((mavlink_message_t *)message)->len);
    // int i;

    // printf("payload:\n");
    // for (i=0;i<10;i++){
    //     printf("%8x ",((mavlink_message_t *)message)->payload64[i]);
    // }
    // printf("\n");
    //printf("fifo out :id:%d\n",id);

    gettimeofday(&tv,&tz);
    p=localtime(&tv.tv_sec);
    uint8_t second =p->tm_sec;
    int msec = tv.tv_usec;
    printf("fifo out :msgid:%d,time: %d:%d\n",message.msgid,second,msec);

    //printf("tid=%lu\n", gettid());

    stdMavconnInterface -> send_message(& message);

    if(flag_Aperiodic==1)
    {
        flag_Aperiodic=0;
        if(flag_Time==1)
        {
           flag_Time=0;
           Common_Timer->TimeOut();
        }
    }

}

// void AugMavConnInterface::send_message(const mavlink_message_t *message,const uint8_t msgid,const uint8_t msgLength){
//     stdMavconnInterface -> send_message(message);
// }
// void AugMavConnInterface::send_message(const mavlink_message_t *message, uint8_t sysid, uint8_t compid,const uint8_t msgid,const uint8_t msgLength){
//     stdMavconnInterface-> send_message(message,sysid,compid);
// }

void AugMavConnInterface::send_bytes(const uint8_t *bytes, size_t length){
    stdMavconnInterface -> send_bytes(bytes, length);
}




mavlink_status_t AugMavConnInterface::get_status(){
    return stdMavconnInterface->get_status();
}

/**
Deprecated in newer veresion
*/
// void set_protocol_version(Protocol pver){
//     stdMavconnInterface -> set_protocol_version(pver);
// }

// Protocol get_protocol_version(){
//     return stdMavconnInterface->get_protocol_version();
// }

void AugMavConnInterface::close(){
    // TODO do something. seems like nothing to do?
}

AugMavConnInterface::~AugMavConnInterface(){
    // TODO do something. seems like nothing to do?
}
